package org.rowan.adi;

/**
 * <p>The CoordinateConversion class is able to take in coordinates from a given
 * coordinate system and convert them into coordinates for another specified
 * coordinate system.</p>
 *
 *<p>A conversion factor is used to describe the distance in meters for every
 * cartesian unit. Here are some example of conversion factors: <ul>
 * <li>1000 -> 1 kilometers</li>
 * <li>2.3 -> 2.3 meters</li>
 * <li>0.5 -> 0.5 meters</li></ul></p>
 *
 * @author Spence DiNicolantonio, Jonathan Palka, Robert Russell
 * @version 1.0
 */
public class CoordinateConversion {

    private double conversionFactor; // conversion factor based on meters

    /**
     * Creates an instance of CoordinateConversion with a given conversion factor.
     * @param conversionFactor the conversion factor to be used in conversion computations.
     * @throws IllegalArgumentException the conversion factor must be positive.
     */
    public CoordinateConversion(double conversionFactor) {
        if (conversionFactor <= 0) {
            throw new IllegalArgumentException("The conversion factor must be positive.");
        }
        this.conversionFactor = conversionFactor;
    }

    /**
     * Creates an instance of CoordinateConversion with a conversion factor of 1.
     */
    public CoordinateConversion() {
        this.conversionFactor = 1;
    }

    /**
     * Converts a given set of Cartesian coordinates to WGS-84 coordinates.
     * @param point an instance of Vector representing a location in the cartesian
     * coordinate system in meters multiplied by the conversion factor.
     * @return an array of doubles, which contains the longitude, latitude, and
     * height in meters, in the WGS-84 coordinate system.
     */
    public double[] cartesianToWGS84(Vector point) {
        double x = point.getX() * this.conversionFactor;
        double y = point.getY() * this.conversionFactor;
        double z = point.getZ() * this.conversionFactor;
        double lon = 0;
        double lat = 0;
        double height = 0;
        double a = 6378137; //equatorial radius in meters
        double b = 6356752.3;   //polar radius in meters
        double P = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2));
        double theta = Math.atan((z * a) / (P * b));
        double ePrime2 = (Math.pow(a, 2) - Math.pow(b, 2)) / Math.pow(b, 2);
        double F = (a - b) / a;
        double e2 = 2 * F - Math.pow(F, 2);

        lat = Math.atan((z + ePrime2 * Math.pow(Math.sin(theta), 3)) / (P - e2 * a * Math.pow(Math.cos(theta), 3)));
        double N = a / Math.sqrt(1 - e2 * Math.pow(Math.sin(lat), 2));
        lon = Math.atan2(y, x);
        height = (P / Math.cos(lat)) - N;
        //Converts lattitude and longitude points to degrees.
        double[] coord = {lat * 57.2957795, lon * 57.2957795, height};

        return coord;
    }

    /**
     * Converts a given set of WGS-84 coordinates to Cartesian coordinates.
     * @param longitude the longitude of the coordinate.
     * @param latitude the latitude of the coordinate.
     * @param altitude the altitude of the coordinate in meters.
     * @return an array of doubles, which contains the X, Y, and Z positions in
     * the Cartesian coordinate system in units of meters multiplied by the conversion factor.
     */
    public double[] wgs84ToCartesian(double longitude, double latitude, double altitude) {
        longitude = longitude * 0.0174532925;
        latitude = latitude * 0.0174532925;
        double X = 0;
        double Y = 0;
        double Z = 0;
        double N = 0;
        double F = 0;
        double e2 = 0;
        double a = 6378137; //equatorial radius in meters
        double b = 6356752.3;   //polar radius in meters

        F = (a - b) / a;
        e2 = (2 * F) - Math.pow(F, 2);
        N = a / (Math.sqrt(1 - e2 * Math.pow(Math.sin(latitude), 2)));
        X = ((N + altitude) * Math.cos(latitude) * Math.cos(longitude)) / this.conversionFactor;
        Y = ((N + altitude) * Math.cos(latitude) * Math.sin(longitude)) / this.conversionFactor;
        Z = ((N * (1 - e2) + altitude) * Math.sin(latitude)) / this.conversionFactor;
        double[] coord = {X, Y, Z};

        return coord;
    }
}
